% 重点参考《数控系统连续小线段的路径优化与速度前瞻控制算法研究_聂伟刚》
% T        : 为插补周期
% Amax     : 机床最大的切向速度。切向速度是物体在运动方向上的速度，也可以说是物体沿着曲线运动时的速度。 
%            在直线运动中，切向速度等于物体的速度。
% Anmax    : 机床最大法向速度，法向速度是物体在运动方向的垂直方向上的速度，也可以理解为物体沿着曲线运动时的速度。
% alpha (α): 线段衔接角
% delta (δ): 轮廓误差
% sigma (σ): 弓高误差
% E        : 综合误差
% J        : 加加速度
% A        : 加速度
% V        : 速度
% l        : 线段长度
% S        : 路径长度

% 运动学基本公式推导
% 速度      v = v0 + at
% 路程      s = v0*t + (a*t^2)/2
% 加速度    a = v / t
% 加加速度  jerk = da / dt
%          v^2 = v0^2 + 2as

% Matlbal 的主函数 函数名字与文件名字一样
function cnc()
    v           = [1000, 1000, 500];            %mm/s
    acc         = [10000, 10000, 2000];         %mm/s^2
    L           = 10;                           %mm
    speed       = 300;                          %mm/s
    V0          = 100;                          %初始速度 mm/s
    vi = DirectTransition(v, acc, speed, L, V0);
    fprintf("直接过渡的最大拐角速度: %.6f\r\n", vi);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 直接过度 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Pi-1运动到Pi点改变方向到Pi+1
% x_max_v     : X轴最大速度
% y_max_v     : Y轴最大速度
% z_max_v     : Z轴最大速度
% x_max_acc   : X轴最大加速度
% y_max_acc   : Y轴最大加速度
% z_max_acc   : Z轴最大加速度
% target_v    : Gcode设定的Pi点拐角速度
% l           : Pi-1运动到Pi的距离
% v0          : Pi-1点的速度
function Vi = DirectTransition(max_v, max_acc, target_v, l, v0)
    % + : just
    % - : lose
    % Vi_lose = Vi_just = Vi
    % Vi_lose 和 Vi_just 表示在Pi点的过度前和过度后的速度。
    % Anmax*T = 2 * ( Vi * sin( (pi - alpha) / 2 ) );
    % 推导过程(结合图看)：等腰三角形的高也是中线。Anmax*T就是底边,Vi * sin( (pi - alpha) / 2 )是Anmax*T的一半。
    % 注意：
    % 不需要引入一个会随角度变换的Anmax*T变量,Vi期望拐角速度是给定的
    Vimax = [1,1,1];
    Vimax(1) = min(max_v);
    Vimax(2) = target_v;
    acc = min(max_acc);
    Vimax(3) = sqrt( v0^2 + 2*acc*l );              % 在距离内能达到的最大速度
    Vi = min( Vimax );
end